#line 1 "D:/Archivio/mikroC/tric-sensors/tricopter.c"
#line 16 "D:/Archivio/mikroC/tric-sensors/tricopter.c"
void Soft_UART_Write_Text(char* testo) {
 char cnt;
 for (cnt=0; cnt<strlen(testo); cnt++) {
 Soft_UART_Write(testo[cnt]);
 }
}
#line 25 "D:/Archivio/mikroC/tric-sensors/tricopter.c"
void ADXL345_Wr(unsigned short address, unsigned short in) {
 I2C1_Start();
 I2C1_Wr(0x3A);
 I2C1_Wr(address);
 I2C1_Wr(in);
 I2C1_Stop();
}
#line 35 "D:/Archivio/mikroC/tric-sensors/tricopter.c"
unsigned short ADXL345_Rd(unsigned short address) {
 unsigned short out;
 I2C1_Start();
 I2C1_Wr(0x3A);
 I2C1_Wr(address);
 I2C1_Stop();
 I2C1_Start();
 I2C1_Wr(0x3B);
 out = I2C1_Rd(0);
 I2C1_Stop();
 return out;
}
#line 52 "D:/Archivio/mikroC/tric-sensors/tricopter.c"
void ADXL345_getAcc(int *acc) {
 unsigned short cnt;
 unsigned short acc_tmp[6];
 char z_string[8];

 I2C1_Start();
 I2C1_Wr(0x3A);
 I2C1_Wr(0x32);
 I2C1_Stop();
 I2C1_Start();
 I2C1_Wr(0x3B);
 for (cnt=0; cnt<6; cnt++) {
 char ack;
 ack = (cnt<5);
 acc_tmp[cnt] = I2C1_Rd(ack);
 }
 I2C1_Stop();

 acc[0] = acc_tmp[1];
 acc[0] = acc[0] << 8;
 acc[0] |= acc_tmp[0];
 acc[1] = acc_tmp[3];
 acc[1] = acc[1] << 8;
 acc[1] |= acc_tmp[2];
 acc[2] = acc_tmp[5];
 acc[2] = acc[2] << 8;
 acc[2] |= acc_tmp[4];
}
#line 83 "D:/Archivio/mikroC/tric-sensors/tricopter.c"
void dataArrayInit(int *fifox, int *fifoy, int *fifoz) {
 unsigned short cnt;
 int acc_tmp[3];
 for (cnt=0; cnt< 16 ; cnt++) {
 ADXL345_getAcc(&acc_tmp);
 fifox[cnt] = acc_tmp[0];
 fifoy[cnt] = acc_tmp[1];
 fifoz[cnt] = acc_tmp[2];
 }
}
#line 96 "D:/Archivio/mikroC/tric-sensors/tricopter.c"
void dataArrayUpdate(int *fifox, int *fifoy, int *fifoz) {
 static unsigned short table_pointer;
 int acc_tmp[3];
 if (table_pointer < 0 || table_pointer >=  16 ) table_pointer = 0;
 ADXL345_getAcc(&acc_tmp);
 fifox[table_pointer] = acc_tmp[0];
 fifoy[table_pointer] = acc_tmp[1];
 fifoz[table_pointer] = acc_tmp[2];
 table_pointer++;
}
#line 109 "D:/Archivio/mikroC/tric-sensors/tricopter.c"
int dataArrayFilter(int *fifo) {
 unsigned short cnt;
 long value = 0;
 for (cnt=0; cnt< 16 ; cnt++) {
 value += fifo[cnt];
 }
 return (int)(value/ 16 );
}


void main() {

 int acc_now[3];
 int acc_x[ 16 ];
 int acc_y[ 16 ];
 int acc_z[ 16 ];

 char x_string[8];
 char y_string[8];
 char z_string[8];

  TRISC.B2  = 0;

 ADCON1 = 0x0F;
 CMCON = 0xCF;

 Soft_UART_Init(&PORTB, 6, 7, 38400, 0);
 I2C1_Init(400000);
 Delay_ms(500);

 ADXL345_Wr(0x2C, 0x0B);
 ADXL345_Wr(0x2D, 0x08);
 ADXL345_Wr(0x31, 0x0B);
 Delay_ms(500);

 dataArrayInit(&acc_x, &acc_y, &acc_z);

 while (1) {
  PORTC.B2  = ! PORTC.B2 ;

 dataArrayUpdate(&acc_x, &acc_y, &acc_z);

 IntToStr(dataArrayFilter(&acc_x), x_string);
 IntToStr(dataArrayFilter(&acc_y), y_string);
 IntToStr(dataArrayFilter(&acc_z), z_string);

 Soft_UART_Write_Text("X: ");
 Soft_UART_Write_Text(&x_string);
 Soft_UART_Write_Text(" - Y: ");
 Soft_UART_Write_Text(&y_string);
 Soft_UART_Write_Text(" - Z: ");
 Soft_UART_Write_Text(&z_string);
 Soft_UART_Write(13);
 Soft_UART_Write(10);

 Delay_ms(50);
 }


}
